/*
 * bsp_motion.c
 *
 *  Created on: Mar 1, 2024
 *      Author: SDJX
 */

#include "bsp_motion.h"
#include "bsp.h"




int g_Encoder_All_Now[MAX_MOTOR] = {0};
int g_Encoder_All_Last[MAX_MOTOR] = {0};

int g_Encoder_All_Offset[MAX_MOTOR] = {0};

int g_speed_L1 = 0;		//临时测试使用
int g_speed_L2 = 0;		//临时测试使用
int g_speed_R1 = 0;		//临时测试使用
int g_speed_R2 = 0;		//临时测试使用

uint8_t g_start_ctrl = 0;

car_data_t car_data;
motor_data_t motor_data;
car_type_t g_car_type = CAR_MECANUM;

/**
 * 控制车辆运动，对四个马达分别设置PWM信号。
 * 控制小车运动，Motor_X 的有效范围为 [-3600, 3600]，超出范围则无效。
 * 
 * @param Motor_1 马达1的PWM值。
 * @param Motor_2 马达2的PWM值。
 * @param Motor_3 马达3的PWM值。
 * @param Motor_4 马达4的PWM值。
 */
void Motion_Set_Pwm(int16_t Motor_1, int16_t Motor_2, int16_t Motor_3, int16_t Motor_4)
{
    int16_t max_value = MOTOR_MAX_PULSE - MOTOR_IGNORE_PULSE;   // 定义马达PWM信号的有效最大值，减去一个死区以确保范围控制

    if (Motor_1 >= -max_value && Motor_1 <= max_value)          // 检查并设置马达1的PWM值
    {
        Motor_Set_Pwm(MOTOR_ID_M1, Motor_1);
    }
    if (Motor_2 >= -max_value && Motor_2 <= max_value)          // 检查并设置马达2的PWM值
    {
        Motor_Set_Pwm(MOTOR_ID_M2, Motor_2);
    }
    if (Motor_3 >= -max_value && Motor_3 <= max_value)          // 检查并设置马达3的PWM值
    {
        Motor_Set_Pwm(MOTOR_ID_M3, Motor_3);
    }
    if (Motor_4 >= -max_value && Motor_4 <= max_value)          // 检查并设置马达4的PWM值
    {
        Motor_Set_Pwm(MOTOR_ID_M4, Motor_4);
    }
}

// The car stopped  小车停止
void Motion_Stop(uint8_t brake)
{
    Motion_Set_Speed(0, 0, 0, 0);
    PID_Clear_Motor(MAX_MOTOR);
    Motor_Stop(brake);
    g_start_ctrl = 0;
}

/**
 * 设置电机运动速度
 * 
 * 本函数用于设定四个电机的速度。通过将速度值写入相应的速度设定数组，
 * 并随后调用PID控制器来设定每个电机的目标速度。
 * 
 * @param speed_m1 电机1的速度设定值，单位为脉冲/秒。
 * @param speed_m2 电机2的速度设定值，单位为脉冲/秒。
 * @param speed_m3 电机3的速度设定值，单位为脉冲/秒。
 * @param speed_m4 电机4的速度设定值，单位为脉冲/秒。
 * 
 * 设置速度 speed_mX=[-1000, 1000], 单位为：mm/s
 * Set speed speed mX=[-1000, 1000], unit: mm/s
 */
void Motion_Set_Speed(int16_t speed_m1, int16_t speed_m2, int16_t speed_m3, int16_t speed_m4)
{
    g_start_ctrl = 1;       // 启动控制标志，设置为1表示开始控制电机速度
    // 设置四个电机的速度,将输入的速度值分别赋给四个电机的速度设定数组
    motor_data.speed_set[0] = speed_m1;
    motor_data.speed_set[1] = speed_m2;
    motor_data.speed_set[2] = speed_m3;
    motor_data.speed_set[3] = speed_m4;

    // 遍历所有电机，为每个电机设置速度目标值
    for (uint8_t i = 0; i < MAX_MOTOR; i++)
    {
        PID_Set_Motor_Target(i, motor_data.speed_set[i] * 1.0);   // 调整PID控制器的电机目标速度，速度值乘以1.0以保持单位一致性
    }
}


/**
 * 从编码器读取当前每个车轮的速度（以mm/s为单位）单位mm/s
 * 
 * @param car 指向car_data_t结构体的指针，用于存储车轮速度数据
 * 
 * @note 该函数会根据轮子间距和编码器分辨率计算车轮速度
 * @note 函数的主要功能是从编码器数据中计算出车辆每个轮子的当前速度
 * 并进一步推算出车辆在XYZ坐标系下的线性速度和角速度。
 * 若控制逻辑已启动，还会更新电机速度数据并执行PID控制计算
 */
void Motion_Get_Speed(car_data_t* car)
{
    // 获取当前编码器的值
    Motion_Get_Encoder();

    float circle_mm = Motion_Get_Circle_MM();   // 获取车轮转动一周的毫米数 400.203mm

    // 计算每个车轮的速度
    float speed_m1 = (g_Encoder_All_Offset[0]) * 100 * circle_mm / (float)ENCODER_CIRCLE;
    float speed_m2 = (g_Encoder_All_Offset[1]) * 100 * circle_mm / (float)ENCODER_CIRCLE;
    float speed_m3 = (g_Encoder_All_Offset[2]) * 100 * circle_mm / (float)ENCODER_CIRCLE;
    float speed_m4 = (g_Encoder_All_Offset[3]) * 100 * circle_mm / (float)ENCODER_CIRCLE;
    
    float robot_APB = Motion_Get_APB();         // 获取机器人APB（Advanced Process Block）的值

//    g_speed_L1 = speed_m1;	//测试用		//打印当前速度值 1374, 2693, 2748, 2693
//    g_speed_L2 = speed_m2;	//测试用
//    g_speed_R1 = speed_m3;	//测试用
//    g_speed_R2 = speed_m4;	//测试用

    // 计算机器人的X、Y、Z方向的速度
    car->Vx = (speed_m1 + speed_m2 + speed_m3 + speed_m4) / 4;
	car->Vy = -(speed_m1 - speed_m2 - speed_m3 + speed_m4) / 4;
    car->Vz = -(speed_m1 + speed_m2 - speed_m3 - speed_m4) / 4.0f / robot_APB * 1000;

//	car->Vy = (speed_m1 + speed_m2 - speed_m3 - speed_m4) / 4;


    // 如果控制开始，则更新马达速度并进行PID计算
    if (g_start_ctrl)
    {
        motor_data.speed_mm_s[0] = speed_m1;
        motor_data.speed_mm_s[1] = speed_m2;
        motor_data.speed_mm_s[2] = speed_m3;
        motor_data.speed_mm_s[3] = speed_m4;
        PID_Calc_Motor(&motor_data);
    }
}

// Returns half of the sum of the current cart wheel axles  返回当前小车轮子轴间距和的一半
float Motion_Get_APB(void)
{
    return MECANUM_MINI_APB;
}

// Returns the number of millimeters at which the current wheel has been turned  返回当前小车轮子转一圈多少毫米
float Motion_Get_Circle_MM(void)
{
    return MECANUM_MINI_CIRCLE_MM;
}

// 返回当前控制的小车类型。
uint8_t Motion_Get_Car_Type(void)
{
    return (uint8_t)g_car_type;
}


// Obtain encoder data and calculate the number of deviation pulses  获取编码器数据，并计算偏差脉冲数
void Motion_Get_Encoder(void)
{
    Encoder_Update_Count();                 //更新编码器计数
    Encoder_Get_ALL(g_Encoder_All_Now);     //获取当前所有编码器的计数值

    for(uint8_t i = 0; i < MAX_MOTOR; i++)
    {
        g_Encoder_All_Offset[i] = g_Encoder_All_Now[i] - g_Encoder_All_Last[i]; //计算编码器的增量值，用于计算速度
	    g_Encoder_All_Last[i] = g_Encoder_All_Now[i];
    }
//    g_speed_L1 = g_Encoder_All_Offset[0];		//测试用		//打印当前速度值 :23, 53, 50, 52
//    g_speed_L2 = g_Encoder_All_Offset[1];		//测试用
//    g_speed_R1 = g_Encoder_All_Offset[2];		//测试用
//    g_speed_R2 = g_Encoder_All_Offset[3];		//测试用
}

/**
 * 控制小车的运动
 * 
 * 通过对小车在X、Y、Z三个方向上的速度进行控制，来实现小车的前进、后退、左右转弯以及旋转。
 * X轴的速度决定了小车的前进与后退，Y轴的速度决定了小车的左转与右转，Z轴的速度决定了小车的旋转。
 * 
 * @param V_x 小车在X轴方向的速度，单位为毫米/秒。正值表示前进，负值表示后退。
 * @param V_y 小车在Y轴方向的速度，单位为毫米/秒。正值表示左转，负值表示右转。
 * @param V_z 小车在Z轴方向的速度，单位为度/秒。正值表示顺时针旋转，负值表示逆时针旋转。
 */
void Motion_Ctrl(int16_t V_x, int16_t V_y, int16_t V_z)
{
    
    float robot_APB = Motion_Get_APB();             // 获取当前小车的轴间距---目前设置的是164，需要修改
    float speed_lr = -V_y;                          // 计算左右轮的速度差，用于实现小车的转弯
    float speed_fb = V_x;                           // 计算小车前进后退的速度
    float speed_spin = -V_z / 1000.0f * robot_APB;  // 计算小车旋转的速度，速度值需要根据APB进行调整
    
    // 如果三个输入速度均为0，则停止小车运动，并返回
    if (V_x == 0 && V_y == 0 && V_z == 0)
    {
        Motion_Stop(STOP_BRAKE);
        return;
    }
	#if CAR1
    // 根据计算得到的速度差和前进后退速度，设置四个轮子的速度（主要是转向的方式，这个轮式及履带式）
    int speed_L1_setup = speed_fb - speed_lr + speed_spin;
    int speed_L2_setup = speed_fb - speed_lr + speed_spin;
    int speed_R1_setup = speed_fb + speed_lr - speed_spin;
    int speed_R2_setup = speed_fb + speed_lr - speed_spin;

    //如果有多种机型，可以再增加CAR2/CAR3，可共用一套代码进行编译
	#elif CAR2
	#else
    // 根据计算得到的速度差和前进后退速度，设置四个轮子的速度（主要是转向的方式，这个主要是麦轮，自动机器人用）
    int speed_L1_setup = speed_fb + speed_lr + speed_spin;
    int speed_L2_setup = speed_fb - speed_lr + speed_spin;
    int speed_R1_setup = speed_fb - speed_lr - speed_spin;
    int speed_R2_setup = speed_fb + speed_lr - speed_spin;
	#endif
    // 限制轮子速度的范围，防止速度过大导致小车失控
    if(CAR_PLUS_MAX_SPEED == 700)
    {

    	if (speed_L1_setup > CAR_PLUS_MAX_SPEED) speed_L1_setup = CAR_PLUS_MAX_SPEED;
		if (speed_L1_setup < -CAR_PLUS_MAX_SPEED) speed_L1_setup = -CAR_PLUS_MAX_SPEED;
		if (speed_L2_setup > CAR_PLUS_MAX_SPEED) speed_L2_setup = CAR_PLUS_MAX_SPEED;
		if (speed_L2_setup < -CAR_PLUS_MAX_SPEED) speed_L2_setup = -CAR_PLUS_MAX_SPEED;
		if (speed_R1_setup > CAR_PLUS_MAX_SPEED) speed_R1_setup = CAR_PLUS_MAX_SPEED;
		if (speed_R1_setup < -CAR_PLUS_MAX_SPEED) speed_R1_setup = -CAR_PLUS_MAX_SPEED;
		if (speed_R2_setup > CAR_PLUS_MAX_SPEED) speed_R2_setup = CAR_PLUS_MAX_SPEED;
		if (speed_R2_setup < -CAR_PLUS_MAX_SPEED) speed_R2_setup = -CAR_PLUS_MAX_SPEED;
    }
    else
    {
    	if (speed_L1_setup > 1000) speed_L1_setup = 1000;
		if (speed_L1_setup < -1000) speed_L1_setup = -1000;
		if (speed_L2_setup > 1000) speed_L2_setup = 1000;
		if (speed_L2_setup < -1000) speed_L2_setup = -1000;
		if (speed_R1_setup > 1000) speed_R1_setup = 1000;
		if (speed_R1_setup < -1000) speed_R1_setup = -1000;
		if (speed_R2_setup > 1000) speed_R2_setup = 1000;
		if (speed_R2_setup < -1000) speed_R2_setup = -1000;
    }

    
    // 设置四个轮子的设定实际速度
    Motion_Set_Speed(speed_L1_setup, speed_L2_setup, speed_R1_setup, speed_R2_setup);

//    g_speed_L1 = speed_L1_setup;	//测试用		//这个值通过串口打印值均一至，是这个函数Motion_Ctrl(-200,0,0)里的值
//    g_speed_L2 = speed_L2_setup;	//测试用
//    g_speed_R1 = speed_R1_setup;	//测试用
//    g_speed_R2 = speed_R2_setup;	//测试用
}


// 运动控制句柄，每10ms调用一次，主要处理速度相关的数据
// Motion control handle, called every 10ms, mainly processing speed related data
/**
 * 处理运动控制逻辑。
 * 
 * 本函数负责获取车辆的当前速度数据，并根据启动控制标志来决定是否设置电机的PWM值。
 * 获取速度数据有助于运动控制的精确计算，而PWM值的设置直接控制电机的转速。
 * 
 * 注意：函数中注释掉的代码是用于测试的，目的是为了监控电机PWM值的变化。
 */
void Motion_Handle(void)
{
    // 获取车辆的速度数据
    Motion_Get_Speed(&car_data);

    if (g_start_ctrl)
    {
        // 设置电机的PWM值，以控制电机的转速
        Motion_Set_Pwm(motor_data.speed_pwm[0], motor_data.speed_pwm[1], motor_data.speed_pwm[2], motor_data.speed_pwm[3]);

//		g_speed_L1 = motor_data.speed_pwm[0];	//测试用		//打印马达设定-PWM值，这个值通过串口打印-3600, -3599, -3600, -3596  M1还是会慢
//		g_speed_L2 = motor_data.speed_pwm[1];	//测试用
//		g_speed_R1 = motor_data.speed_pwm[2];	//测试用
//		g_speed_R2 = motor_data.speed_pwm[3];	//测试用
    }
}

